/**
  *   ������CAN�жϽ��պ��������յ������,CAN���ͺ������͵���������Ƶ��.  
**/
#include "CAN_receive.h"
#include "main.h"
//can中断接受数据，接收电机数据，can发送函数发送电机电流控制电机
extern CAN_HandleTypeDef    hcan1;    //CAN串口1
//extern CAN_HandleTypeDef    hcan2;
static motor_measure_t      motor_chassis[7];
/* 添加圈数计数相关静态变量*/
//static int16_t ecd_last[7] = {0};          // 上次编码器值
//static int32_t total_ecd_last[7] = {0};    // 上次总编码器值

//static CAN_TxHeaderTypeDef  gimbal_tx_message;
//static int8_t              gimbal_can_send_data[8];
static CAN_TxHeaderTypeDef  chassis_tx_message;
static int8_t              chassis_can_send_data[8];


// 电机数据的读取
#define get_motor_measure(ptr, data)                                    \
do {                                                             \
(ptr)->last_ecd = (ptr)->ecd;                                   \
(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
(ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);      \
(ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);  \
(ptr)->temperate = (data)[6];                                   \
/* 添加圈数计算 */                                               \
if((ptr)->last_ecd - (ptr)->ecd > 4096) {                       \
(ptr)->round_count++;                                       \
} else if((ptr)->ecd - (ptr)->last_ecd > 4096) {                \
(ptr)->round_count--;                                       \
}                                                               \
(ptr)->total_ecd = (ptr)->round_count * 8192 + (ptr)->ecd;      \
(ptr)->total_angle = (ptr)->total_ecd * 360.0f / 8192.0f;       \
} while(0)


//hal 库can回调函数，接收电机数据
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    CAN_RxHeaderTypeDef rx_header;
    uint8_t rx_data[8];

    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);

    switch (rx_header.StdId)
    {
        case CAN_3508_M1_ID:
        case CAN_3508_M2_ID:
        case CAN_3508_M3_ID:
        case CAN_3508_M4_ID:
        case CAN_YAW_MOTOR_ID:
        case CAN_PIT_MOTOR_ID:
        case CAN_TRIGGER_MOTOR_ID:
        {
            static uint8_t i = 0;
            //get motor id
            i = rx_header.StdId - CAN_3508_M1_ID;
            get_motor_measure(&motor_chassis[i], rx_data);
            break;
        }
        default:
        {
            break;
        }
    }
		HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
}


void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
{
    uint32_t send_mail_box;
    chassis_tx_message.StdId = 0x200;
    chassis_tx_message.IDE = CAN_ID_STD;
    chassis_tx_message.RTR = CAN_RTR_DATA;
    chassis_tx_message.DLC = 0x08;
    chassis_can_send_data[0] = motor1 >> 8;
    chassis_can_send_data[1] = motor1 ;
    chassis_can_send_data[2] = motor2 >> 8;
    chassis_can_send_data[3] = motor2 ;
    chassis_can_send_data[4] = motor3 >> 8;
    chassis_can_send_data[5] = motor3 ;
    chassis_can_send_data[6] = motor4 >> 8;
    chassis_can_send_data[7] = motor4 ;

    HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message, chassis_can_send_data, &send_mail_box);

}

/*
void CAN_cmd_gimbal(int16_t yaw, int16_t pitch, int16_t shoot, int16_t rev)
{
    uint32_t send_mail_box;
    gimbal_tx_message.StdId = CAN_GIMBAL_ALL_ID;
    gimbal_tx_message.IDE = CAN_ID_STD;
    gimbal_tx_message.RTR = CAN_RTR_DATA;
    gimbal_tx_message.DLC = 0x08;
    gimbal_can_send_data[0] = (yaw >> 8);
    gimbal_can_send_data[1] = yaw;
    gimbal_can_send_data[2] = (pitch >> 8);
    gimbal_can_send_data[3] = pitch;
    gimbal_can_send_data[4] = (shoot >> 8);
    gimbal_can_send_data[5] = shoot;
    gimbal_can_send_data[6] = (rev >> 8);
    gimbal_can_send_data[7] = rev;
    HAL_CAN_AddTxMessage(&GIMBAL_CAN, &gimbal_tx_message, gimbal_can_send_data, &send_mail_box);
}
*/
/*
//发送ID为0x700的can包，它会设置3508电机进入快速设置ID
void CAN_cmd_chassis_reset_ID(void)
{
    uint32_t send_mail_box;
    chassis_tx_message.StdId = 0x700;
    chassis_tx_message.IDE = CAN_ID_STD;
    chassis_tx_message.RTR = CAN_RTR_DATA;
    chassis_tx_message.DLC = 0x08;
    chassis_can_send_data[0] = 0;
    chassis_can_send_data[1] = 0;
    chassis_can_send_data[2] = 0;
    chassis_can_send_data[3] = 0;
    chassis_can_send_data[4] = 0;
    chassis_can_send_data[5] = 0;
    chassis_can_send_data[6] = 0;
    chassis_can_send_data[7] = 0;

    HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}*/
/*
//���� 2006�������ָ��
const motor_measure_t *get_trigger_motor_measure_point(void)
{
    return &motor_chassis[6];
}
*/
//返回3508数据指针
 motor_measure_t *get_chassis_motor_measure_point(uint8_t i)
{
    return &motor_chassis[(i & 0x03)];
}
/*
//����yaw 6020�������ָ��
const motor_measure_t *get_yaw_gimbal_motor_measure_point(void)
{
    return &motor_chassis[4];
}

//����pitch 6020�������ָ��  
const motor_measure_t *get_pitch_gimbal_motor_measure_point(void)
{
    return &motor_chassis[5];
}
*/
//*******************************************************************�¼�










